#include "StdAfx.h"
#include "Deinterlace.h"

//////////////////////////////////////////////////////////////////////////

static uint32_t Average(uint32_t a, uint32_t b) 
{
	return (a|b) - (((a^b) & 0xfefefefe) >> 1);
}
struct UnpackedPixel 
{
	int r, g, b;
};
static void Unpack(UnpackedPixel& upx, uint32_t px) 
{
	upx.r = (px >> 16) & 0xff;
	upx.g = (px >>  8) & 0xff;
	upx.b = (px >>  0) & 0xff;
}
static uint32_t Pack(const UnpackedPixel& upx) 
{
	return (upx.r << 16) + (upx.g << 8) + upx.b;
}

static void Lerp(UnpackedPixel& dst, const UnpackedPixel& src1, const UnpackedPixel& src2, uint32_t factor256) 
{
	dst.r = src1.r + (((src2.r - src1.r)*factor256 + 128) >> 8);
	dst.g = src1.g + (((src2.g - src1.g)*factor256 + 128) >> 8);
	dst.b = src1.b + (((src2.b - src1.b)*factor256 + 128) >> 8);
}
static int Difference(const UnpackedPixel& p1, const UnpackedPixel& p2) 
{
	int dr = p1.r - p2.r;
	int dg = p1.g - p2.g;
	int db = p1.b - p2.b;
	return dr*dr + dg*dg + db*db;
}
static void elascan(uint32_t *dst, const uint32_t *src1, const uint32_t *src2, uint32_t w, int *tmp) 
{
	uint32_t a = *src1;
	uint32_t b = *src2;

	*dst++ = Average(a, b);

	if (w < 2)
	{
		return;
	}

	w -= 2;

	int *tmp2 = tmp;
	tmp2[0] = 0;
	tmp2[1] = 0;
	tmp2[2] = 0;
	tmp2 += 3;

	UnpackedPixel x0;
	UnpackedPixel x1;
	UnpackedPixel x2;
	UnpackedPixel y0;
	UnpackedPixel y1;
	UnpackedPixel y2;

	const uint32_t *src1a = src1;
	const uint32_t *src2a = src2;
	Unpack(x0, *src1a++);
	Unpack(x1, *src1a++);
	Unpack(y0, *src2a++);
	Unpack(y1, *src2a++);

	int wt = w;
	for(;;) 
	{
		Unpack(x2, *src1a++);
		Unpack(y2, *src2a++);

		tmp2[0] = Difference(x0, y2);
		tmp2[1] = Difference(x1, y1);
		tmp2[2] = Difference(x2, y0);
		tmp2 += 3;

		if (!--wt)
		{
			break;
		}

		Unpack(x0, *src1a++);
		Unpack(y0, *src2a++);

		tmp2[0] = Difference(x1, y0);
		tmp2[1] = Difference(x2, y2);
		tmp2[2] = Difference(x0, y1);
		tmp2 += 3;

		if (!--wt)
		{
			break;
		}

		Unpack(x1, *src1a++);
		Unpack(y1, *src2a++);

		tmp2[0] = Difference(x2, y1);
		tmp2[1] = Difference(x0, y0);
		tmp2[2] = Difference(x1, y2);
		tmp2 += 3;

		if (!--wt)
		{
			break;
		}
	}

	tmp2[0] = 0;
	tmp2[1] = 0;
	tmp2[2] = 0;

	for(uint32_t x=0; x<w; ++x) 
	{
		int sum0 = tmp[0] + tmp[3] + tmp[6];
		int sum1 = tmp[1] + tmp[4] + tmp[7];
		int sum2 = tmp[2] + tmp[5] + tmp[8];

		if (sum0 < sum2) 
		{
			if (sum0 < sum1) 
			{
				a = src1[0];
				b = src2[2];
			} 
			else 
			{
				a = src1[1];
				b = src2[1];
			}
		} 
		else 
		{
			if (sum2 < sum1) 
			{
				a = src1[2];
				b = src2[0];
			} 
			else 
			{
				a = src1[1];
				b = src2[1];
			}
		}

		*dst++ = Average(a, b);
		tmp += 3;
		++src1;
		++src2;
	}

	a = src1[1];
	b = src2[1];

	*dst++ = Average(a, b);
}

class VFBitmapRowIterator 
{
public:
	VFBitmapRowIterator(cv::Mat const& vb)
		: mpData((char *)vb.data + vb.cols * vb.channels() * (vb.rows - 1))
		, mPitch(-vb.cols * vb.channels())
	{
	}

	uint32_t *operator*() { return (uint32_t*)mpData; }
	const uint32_t *operator*() const { return (const uint32_t*)mpData; }

	uint32_t* operator[](uint32_t y) { return (uint32_t *)(mpData + mPitch * y); }
	const uint32_t* operator[](uint32_t y) const { return (const uint32_t *)(mpData + mPitch * y); }

	VFBitmapRowIterator& operator--() { mpData -= mPitch; return *this; }
	VFBitmapRowIterator operator--(int) 
	{
		VFBitmapRowIterator tmp(*this);
		mpData -= mPitch;
		return tmp;
	}
	VFBitmapRowIterator& operator++() { mpData += mPitch; return *this; }
	VFBitmapRowIterator operator++(int) 
	{
		VFBitmapRowIterator tmp(*this);
		mpData += mPitch;
		return tmp;
	}

protected:
	char			*mpData;
	const ptrdiff_t	mPitch;
};


cv::Mat Deinterlacer::deinterlace(Deinterlacer::Method method, cv::Mat const& frame)
{
	if (method == Deinterlacer::Method::NONE)
	{
		return frame;
	}

	Q_ASSERT(frame.channels() == 4);

	if (m_image.cols == 0)
	{
		m_image = frame.clone();
	}

	//bob
	if (method == Deinterlacer::Method::BOB)
	{
		uint32_t pitch = frame.cols;
		uint32_t w = frame.cols;
		uint32_t h = frame.rows;
		uint32_t* dst = (uint32_t*)m_image.data;
		uint32_t const* src = (uint32_t const*)frame.data;

		// Copy first and last lines
		memcpy(dst, src, w*4);
		memcpy((char *)dst + pitch*(h-1), (char *)src + pitch*(h-1), w*4);

		if ((h-=2) > 0) 
		{
			uint32_t const* src2 = (src + pitch);
			uint32_t const* src3 = (src + pitch*2);
			dst = (dst + pitch);

			do 
			{
				uint32_t w2 = w;
				do {
					uint32_t a = *src++;
					uint32_t b = *src2++;
					uint32_t c = *src3++;

					*dst++	= (((((a>>1)&0x7f7f7f) + ((c&0xfefefe)>>1) + (a&b&0x010101))>>1)&0x7f7f7f) + ((b&0xfefefe)>>1);
				} while(--w2);
			} while(--h);
		}
	}
	else if (method == Deinterlacer::Method::ELA) //ela
	{
		uint32_t w = m_image.cols;
		uint32_t h = m_image.rows;

		m_temp_buffer.resize(w * 3);

		VFBitmapRowIterator srcIt(frame);
		VFBitmapRowIterator dstIt(m_image);

		bool odd = true;

		memcpy(*dstIt++, srcIt[odd ? 1 : 0], 4*w);

		for(uint32_t y = 1; y < h - 1; ++y) 
		{
			bool scanOdd = (bool)(y & 1);
			if (scanOdd == odd)
			{
				memcpy(*dstIt++, srcIt[y], 4*w);
			}
			else
			{
				elascan(*dstIt++, srcIt[y-1], srcIt[y+1], w, m_temp_buffer.data());
			}
		}

		memcpy(*dstIt++, srcIt[odd ? h-1 : h-2], 4*w);
	}
	else
	{
		return frame;
	}

	return m_image;
}
